#include "vehicle_initializer.h"

#include "infrastructure/vehicle/mklm/mklm_server_manager.h"
#include "infrastructure/vehicle/mklm/mklm_status_provider.h"
#include "infrastructure/vehicle/seer/seer_command_impl.h"
#include "infrastructure/vehicle/seer/seer_status_provider.h"
#include "infrastructure/simulation/sim_agv_command.h"
#include "infrastructure/simulation/sim_status_provider.h"
#include "application/simulation/simulation_service.h"
#include "kernel/agv/agv_entity.h"

namespace application {
namespace vehicle {

VehicleInitializer::VehicleInitializer(
    std::shared_ptr<infrastructure::services::CommonService> commonService,
    std::shared_ptr<VehicleService> vehicleService)
    : m_commonService(commonService)
    , m_vehicleService(vehicleService)
    , m_simulationService(nullptr) {
  if (m_commonService) {
    m_logger = m_commonService->getLogger("VehicleInitializer");
  }
}

void VehicleInitializer::setSimulationService(
    std::shared_ptr<simulation::SimulationService> simulationService) {
  m_simulationService = simulationService;
}

bool VehicleInitializer::initializeVehiclesFromConfig() {
  if (!m_commonService) {
    return false;
  }

  // 1. 首先启动MKLM共享服务器（用于动态注册MKLM车辆）
  auto* mklmServer =
      infrastructure::vehicle::mklm::MklmServerManager::instance();
  if (!mklmServer->isRunning()) {
    quint16 mklmPort = 29204;  // MKLM共享端口
    auto mklmLogger = m_commonService->getLogger("MklmServer");
    bool mklmSuccess = mklmServer->initialize(mklmPort, mklmLogger);

    if (!mklmSuccess) {
      if (m_logger) {
        m_logger->error("MKLM 共享服务器启动失败");
      }
      return false;
    }

    // 连接MKLM服务器的未知AGV信号到VehicleService的动态注册槽
    QObject::connect(mklmServer,
                     &infrastructure::vehicle::mklm::MklmServerManager::
                         unknownMklmAgvDetected,
                     m_vehicleService.get(),
                     &VehicleService::onUnknownMklmAgvDetected,
                     Qt::QueuedConnection);
  }

  // 2. 从配置文件读取并初始化SEER类型车辆（MKLM车辆将动态注册）
  auto config = m_commonService->configuration();
  if (!config) {
    if (m_logger) {
      m_logger->error("无法获取配置服务");
    }
    return false;
  }

  // 获取 AGV 配置节
  auto agvsSectionOpt = config->getSection("agvs");
  if (!agvsSectionOpt.has_value()) {
    if (m_logger) {
      m_logger->warning("配置文件中未找到 agvs 配置节");
    }
    return false;
  }

  auto agvsSection = agvsSectionOpt.value();

  if (m_logger) {
    m_logger->info("开始从配置文件初始化车辆");
  }

  // 获取数组长度
  int agvsCount = agvsSection->getInt("length", 0);
  if (agvsCount == 0) {
    if (m_logger) {
      m_logger->warning("配置中没有找到任何AGV配置");
    }
    return true;  // 没有配置也算成功
  }

  if (m_logger) {
    m_logger->info(QString("发现 %1 个AGV配置").arg(agvsCount));
  }

  int successCount = 0;
  int failureCount = 0;
  int mklmSkippedCount = 0;

  // 遍历数组元素
  for (int i = 0; i < agvsCount; ++i) {
    QString agvPrefix = QString("%1").arg(i);

    auto agvItemSectionOpt = agvsSection->getSection(agvPrefix);
    if (!agvItemSectionOpt.has_value()) {
      if (m_logger) {
        m_logger->warning(QString("无法获取AGV配置索引: %1").arg(i));
      }
      failureCount++;
      continue;
    }

    auto agvItemSection = agvItemSectionOpt.value();

    int agvNumber = agvItemSection->getInt("agvNumber", -1);
    QString name = agvItemSection->getString("name", "");
    QString ip = agvItemSection->getString("ip", "");
    QString typeStr = agvItemSection->getString("type", "");

    if (agvNumber < 0 || ip.isEmpty() || typeStr.isEmpty()) {
      if (m_logger) {
        m_logger->warning(QString("跳过无效的AGV配置: 索引=%1").arg(i));
      }
      failureCount++;
      continue;
    }

    // MKLM类型跳过（将由动态注册处理）
    if (typeStr == "MKLM") {
      if (m_logger) {
        m_logger->info(
            QString("跳过 MKLM AGV %1（将动态注册）").arg(agvNumber));
      }
      mklmSkippedCount++;
      continue;
    }

    if (m_logger) {
      m_logger->info(QString("初始化 AGV: %1, 类型: %2, IP: %3")
                         .arg(agvNumber)
                         .arg(typeStr)
                         .arg(ip));
    }

    bool success = false;
    if (typeStr == "SEER") {
      // SEER AGV: 默认轮询间隔 2000ms
      int pollInterval = 2000;
      success = initializeSeerAgv(agvNumber, ip, pollInterval);
    } else {
      if (m_logger) {
        m_logger->error(QString("未知的 AGV 类型: %1").arg(typeStr));
      }
    }

    if (success) {
      successCount++;
    } else {
      failureCount++;
    }
  }

  if (m_logger) {
    m_logger->info(
        QString(
            "车辆初始化完成: SEER成功 %1, 失败 %2, MKLM跳过 %3（等待动态注册）")
            .arg(successCount)
            .arg(failureCount)
            .arg(mklmSkippedCount));
  }

  // 3. 初始化仿真车辆（如果启用）
  int simSuccessCount = initializeSimulationVehicles();
  if (simSuccessCount > 0 && m_logger) {
    m_logger->info(QString("仿真车辆初始化成功: %1 辆").arg(simSuccessCount));
  }

  return failureCount == 0;
}

bool VehicleInitializer::initializeSeerAgv(int agvNumber, const QString& ip,
                                           int pollInterval) {
  // 创建 SEER 命令实现
  auto logger = m_commonService->getLogger(
      QString("SeerCmd_AGV%1").arg(agvNumber).toStdString());
  auto commandImpl =
      std::make_shared<infrastructure::vehicle::seer::SeerCommandImpl>(logger);

  // 注册 SEER AGV 连接（使用 robokit 端口）
  if (!commandImpl->registerAgv(agvNumber, ip)) {
    if (m_logger) {
      m_logger->error(QString("初始化 SEER AGV %1 命令连接失败: IP=%2")
                          .arg(agvNumber)
                          .arg(ip));
    }
    return false;
  }

  // 注册车辆
  auto registerResult =
      m_vehicleService->registerVehicle(
          agvNumber,
          commandImpl,
          kernel::agv::AgvType::SEER);
  if (!registerResult.isSuccess()) {
    if (m_logger) {
      m_logger->error(QString("注册 SEER AGV %1 失败: %2")
                          .arg(agvNumber)
                          .arg(registerResult.getErrorMessage()));
    }
    return false;
  }

  // 创建 SEER 状态提供者（使用标准 robokit 端口 19204）
  quint16 agvPort = 19204;
  auto statusLogger = m_commonService->getLogger(
      QString("SeerStatus_AGV%1").arg(agvNumber).toStdString());
  auto statusProvider =
      std::make_shared<infrastructure::vehicle::seer::SeerStatusProvider>(
          agvNumber, ip, agvPort, pollInterval,
          10,  // detailPollRatio
          statusLogger);

  // 注册状态提供者
  auto statusResult = m_vehicleService->registerVehicleStatusProvider(
      agvNumber, statusProvider);
  if (!statusResult.isSuccess()) {
    if (m_logger) {
      m_logger->error(QString("注册 SEER AGV %1 状态提供者失败: %2")
                          .arg(agvNumber)
                          .arg(statusResult.getErrorMessage()));
    }
    m_vehicleService->unregisterVehicle(agvNumber);
    return false;
  }

  if (m_logger) {
    m_logger->info(QString("SEER AGV %1 初始化成功，IP: %2:%3, 轮询间隔: %4ms")
                       .arg(agvNumber)
                       .arg(ip)
                       .arg(agvPort)
                       .arg(pollInterval));
  }

  return true;
}

int VehicleInitializer::initializeSimulationVehicles() {
  // 检查是否有仿真服务
  if (!m_simulationService || !m_simulationService->isEnabled()) {
    return 0;
  }

  // 从配置读取仿真车辆
  auto config = m_commonService->configuration();
  if (!config) {
    if (m_logger) {
      m_logger->error("无法获取配置服务");
    }
    return 0;
  }

  // 读取 simulation 配置节
  auto simSectionOpt = config->getSection("simulation");
  if (!simSectionOpt.has_value()) {
    if (m_logger) {
      m_logger->warning("配置文件中未找到 simulation 配置节");
    }
    return 0;
  }

  auto simSection = simSectionOpt.value();

  // 检查是否启用
  bool enabled = simSection->getBool("enabled", false);
  if (!enabled) {
    if (m_logger) {
      m_logger->info("仿真模式未启用");
    }
    return 0;
  }

  // 读取 vehicles 数组
  auto vehiclesSectionOpt = simSection->getSection("vehicles");
  if (!vehiclesSectionOpt.has_value()) {
    if (m_logger) {
      m_logger->warning("simulation 配置中未找到 vehicles 数组");
    }
    return 0;
  }

  auto vehiclesSection = vehiclesSectionOpt.value();
  int vehiclesCount = vehiclesSection->getInt("length", 0);

  if (vehiclesCount == 0) {
    if (m_logger) {
      m_logger->info("simulation.vehicles 配置为空");
    }
    return 0;
  }

  if (m_logger) {
    m_logger->info(QString("发现 %1 个仿真车辆配置").arg(vehiclesCount));
  }

  int successCount = 0;

  // 遍历车辆配置
  for (int i = 0; i < vehiclesCount; ++i) {
    QString vehiclePrefix = QString("%1").arg(i);
    auto vehicleItemOpt = vehiclesSection->getSection(vehiclePrefix);

    if (!vehicleItemOpt.has_value()) {
      if (m_logger) {
        m_logger->warning(QString("无法获取仿真车辆配置索引: %1").arg(i));
      }
      continue;
    }

    auto vehicleItem = vehicleItemOpt.value();

    int agvNumber = vehicleItem->getInt("agvNumber", -1);
    QString name = vehicleItem->getString("name", "");
    QString initialStation = vehicleItem->getString("initialStation", "");
    QString initialMap = vehicleItem->getString("initialMap", "");

    if (agvNumber < 0 || initialStation.isEmpty() || initialMap.isEmpty()) {
      if (m_logger) {
        m_logger->warning(
            QString("跳过无效的仿真车辆配置: 索引=%1").arg(i));
      }
      continue;
    }

    if (m_logger) {
      m_logger->info(
          QString("初始化仿真 AGV: %1, 名称: %2, 初始站点: %3, 地图: %4")
              .arg(agvNumber)
              .arg(name)
              .arg(initialStation)
              .arg(initialMap));
    }

    int batteryPercent = vehicleItem->getInt("batteryLevel", 90);

    bool success =
        initializeSimulationAgv(agvNumber, name, initialStation, initialMap, batteryPercent);

    if (success) {
      successCount++;
    }
  }

  return successCount;
}

bool VehicleInitializer::initializeSimulationAgv(int agvNumber,
                                                  const QString& name,
                                                  const QString& initialStation,
                                                  const QString& initialMap,
                                                  int initialBatteryPercent) {
    if (!m_simulationService || !m_simulationService->engine()) {
        if (m_logger) {
            m_logger->error("仿真引擎未初始化");
        }
        return false;
  }

  auto* engine = m_simulationService->engine();
    auto logger =
        m_commonService->getLogger(QString("SimAgv_%1").arg(agvNumber).toStdString());

    // 1. 在物理模拟器中注册车辆
    engine->physicsSimulator()->registerVehicle(
        agvNumber,
        initialStation.toStdString(),
        initialMap.toStdString(),
        initialBatteryPercent);

    if (m_simulationService) {
        m_simulationService->recordInitialVehicleState(agvNumber, initialStation, initialMap);
    }

  // 2. 在仿真引擎中注册车辆（用于自动更新）
  engine->registerVehicle(agvNumber);

  // 3. 创建仿真命令适配器
  auto commandImpl =
      std::make_shared<simulation::SimAgvCommand>(agvNumber, engine, logger);

  // 4. 注册车辆到 VehicleService
  auto registerResult = m_vehicleService->registerVehicle(
      agvNumber, commandImpl, kernel::agv::AgvType::MKLM);  // 使用 MKLM 类型标记

  if (!registerResult.isSuccess()) {
    if (m_logger) {
      m_logger->error(QString("注册仿真 AGV %1 失败: %2")
                          .arg(agvNumber)
                          .arg(registerResult.getErrorMessage()));
    }
    return false;
  }

  // 5. 创建仿真状态提供者
  auto statusProvider = std::make_shared<simulation::SimStatusProvider>(
      agvNumber, engine, logger);

  // 6. 注册状态提供者
  auto statusResult =
      m_vehicleService->registerVehicleStatusProvider(agvNumber, statusProvider);

  if (!statusResult.isSuccess()) {
    if (m_logger) {
      m_logger->error(QString("注册仿真 AGV %1 状态提供者失败: %2")
                          .arg(agvNumber)
                          .arg(statusResult.getErrorMessage()));
    }
    m_vehicleService->unregisterVehicle(agvNumber);
    return false;
  }

  // 7. 启动状态提供者
  statusProvider->start();

  // 8. 初始化车辆实体的初始状态
  auto* agv = m_vehicleService->getVehicle(agvNumber);
    if (agv) {
        agv->setCurrentMap(initialMap.toStdString());
        agv->setCurrentStation(initialStation.toStdString());
        agv->setOnlineStatus(true);
        agv->setBatteryLevel(initialBatteryPercent);  // 初始电量
        agv->setControlledByDispatcher(false);
        agv->setBusyStatus(false);
        agv->setChargingStatus(false);
        agv->setLockedStatus(false);
        agv->setNavStatus(kernel::agv::NavStatus::None);
    agv->setTaskType(kernel::agv::TaskType::None);
  }

  if (m_logger) {
    m_logger->info(
        QString("仿真 AGV %1 初始化成功，初始站点: %2, 地图: %3")
            .arg(agvNumber)
            .arg(initialStation)
            .arg(initialMap));
  }

  return true;
}

}  // namespace vehicle
}  // namespace application
